import os
import queue
import threading
import time
import logging
from queue import Queue
import struct
import socket

import numpy as np
import lupa
import sys

import modbus_tk.defines as cst
import modbus_tk.modbus_tcp as modbus_tcp
from modbus_tk import modbus_rtu

from _path_planner import LinearPlanner
from _exceptions import RobotNotConnectedError, MotionPlanningError
import _database

robot_logger = logging.getLogger("Controller-Driver")
robot_logger.setLevel(logging.DEBUG)
os.environ['DISPLAY'] = ':1'

GB_dict = {  
    f"GB{i:02d}": {"address": f"{i-1}", "value": False}  
    for i in range(1, 43)  
}  

GI_dict = {  
    f"GI{i:02d}": {"address": f"{300 + (i-1)}", "value": 0}  
    for i in range(1, 43)  
}  

GD_dict = {  
    f"GD{i:02d}": {"address": f"{400 + (i-1)*2}", "value": 0.0}  
    for i in range(1, 43)  
}

GP_dict = {  
    f"GP{i:02d}": {"address": f"{500 + (i-1)*2}", "value": 00.00}  
    for i in range(1, 43)  
} 


#### 视觉取料  右上--右下----左上--左下
### 过渡点
P0 = [0.0,0.0,0.0,0.0,0.0,0.0,198.67,0.0,230.72,0.0,0.0,0.0,0.0]

P26=[27.8, 33.3, 3.8, 0.0, -37.1, -14.0, 208.7, 109.9, 107.3, 0.0, 0.0, 13.8, 0.0]
P27=[27.8, 38.1, 3.6, 0.0, -41.7, -14.0, 208.7, 109.9, 90.3, 0.0, 0.0, 13.8, 0.0]
P28=[-36.3, -1.4, 28.6, 0.0, -27.2, 4.1, 150.3, -110.4, 151.1, 0.0, 0.0, -32.2, 0.0]
P29=[-36.3, 5.3, 31.8, 0.0, -37.1, 4.1, 150.3, -110.4, 124.3, 0.0, 0.0, -32.2, 0.0]

### 回仓2号待取点--取料--待放料--放料
P30=[27.7, 60.4, -40, 0.0, -20.4, -13.9, 255.9, 134.3, 115.9, 0.0, 0.0, 13.8, 0.0]
P31=[27.7, 66.1, -40, 0.0, -26, -13.9, 255.9, 134.3, 90.3, 0.0, 0.0, 13.8, 0.0]
P32=[-51.5, 7.6, 28.2, 0.0, -35.8, -8.3, 119.9, -150.8, 127.7, 0.0, 0.0, -59.8, 0.0]
P33=[-51.5, 13.9, 29.2, 0.0, -43.1, -8.3, 119.9, -150.8, 106.7, 0.0, 0.0, -59.8, 0.0]

### 回仓3号待取点--取料--待放料--放料
P34=[18.9, 48.9, -24.8, 0.0, -24.1, -5.2, 258.7, 88.7, 123, 0.0, 0.0, 13.8, 0.0]
P35=[18.9, 55.6, -24.2, 0.0, -31.4, -5.2, 258.7, 88.7, 92.7, 0.0, 0.0, 13.8, 0.0]
P36=[-64.6, 4.9, 33.2, 0.0, -38.1, -22.8, 79.1, -166.4, 121.9, 0.0, 0.0, -87.4, 0.0]
P37=[-64.6, 12.1, 33.8, 0.0, -46.6, -22.8, 79.1, -166.4, 100.7, 0.0, 0.0, -87.4, 0.0]

### 回仓4号待取点--取料--待放料--放料
P38=[18.8, 35.4, -0.8, 0.0, -34.8, -5.1, 229.5, 78.3, 110.7, 0.0, 0.0, 13.8, 0.0]
P39=[18.8, 41.3, -0.8, 0.0, -40.4, -5.1, 229.5, 78.3, 89.5, 0.0, 0.0, 13.8, 0.0]
P40=[-78.1, -6.8, 43.3, 0.0, -36.5, -35.2, 33.9, -161.2, 125.5, 0.0, 0.0, -113.4, 0.0]
P41=[-78.1, 2.9, 44.3, 0.0, -47.1, -35.2, 33.9, -161.2, 100.3, 0.0, 0.0, -113.4, 0.0]


CL = _database.LocalDatabase()


class VSRController:
    
    def __init__(self, simulate=False):
        self.simulate = simulate

        self.host = "0.0.0.0"
        self.port = 502
        self.modbus_tcp_init()

        ## 初始化状态管理
        self.current_poseRobotA = [0.0] * 6  ## 六轴 J1-J6 
        self.current_poseRobotB = [0.0] * 4  ## 四轴 J1-J4
        self.target_pose = [0.0] * 13   ## 示教点位
        self.speed = 100.0  # mm/s
        self.status = "IDLE"
        self.bo_out =0x00 

        ## 初始化时预填充配置值
        for i in range(6):
            CL.set_value(f'j{i+1}', self.current_poseRobotA[i])
        
        for i in range(4):
            CL.set_value(f'j{i + 7}', self.current_poseRobotB[i])

        self.host = socket.gethostname()
        self._master_init()
        
        self.lock = threading.Lock()
        self.ioa_lock = threading.Lock()  # 新增IOA专用锁

        ## 路径规划
        self.planner = LinearPlanner()
        self.trajectoryRobotA = Queue()
        self.trajectoryRobotB = Queue()

        self._running = True

        self.monitorRegisterControl = threading.Thread(target=self._run)
        self.monitorRegisterControl.start()

        self.ioa_server = IOAServer(self)
        self.algo_th = threading.Thread(target=self.ioa_server.run)
        self.algo_th.start()

        self.condition = threading.Condition()
        self.movement_complete = False

        ## 后台服务
        self._update_thread = threading.Thread(target=self._state_updater)
        self._update_thread.start()


    def _master_init(self):
        self.master_ = modbus_tcp.TcpMaster(host=self.host)


    def modbus_tcp_init(self):
        try:
            self.server = modbus_tcp.TcpServer(port=self.port,address=self.host,
                                               timeout_in_sec=5)
            self.server.start()
            self.register_init()
        
        except Exception as e:
            robot_logger.error(e,exc_info=True)

    def register_init(self,):
        self.slave =self.server.add_slave(1) 
        self.slave.add_block("robot6",cst.HOLDING_REGISTERS, 0, 600) 
        self.slave.add_block("var", cst.COILS, 0, 600)


    def connect(self):
        try:
            if self.simulate:
                self._init_simulator()
            else:
                self._connect_hardware()
            
            # 确保IOA服务在连接后启动
            if not hasattr(self, 'ioa_server'):
                self.ioa_server = IOAServer(self)
                self.algo_th = threading.Thread(target=self.ioa_server.run)
                self.algo_th.start()
            
            self._running = True
            logging.info(f"Controller started in {'SIMULATION' if self.simulate else 'REAL'} mode")
        
        except Exception as e:
            self.stop()
            raise RobotNotConnectedError(f"连接失败: {str(e)}")
    
    ## 设置DO状态
    def SetDO(self, do, state):
        try:
            do_map = {"0": 0, "1": 1, "2": 2, "3": 3, "4": 4, "5":5, "6":6, "7":7}
            do_new = str(do).strip()
            if do_new in do_map:
                with self.ioa_lock:
                    add = do_map[do_new]
                    new_bo = self.bo_count(self.bo_out, add, value=state)
                    self.bo_out = new_bo
                    ## 同步更新到ioa数据发送
                    CL.set_value("BO0", new_bo)  

        except Exception as e:
            print(f"RobotLua class SetDO error is {e}")

    def bo_count(self, BO, bo_num, value):
        # BO 是一个8位的16进制数
        # bo_bit 是第几个IO
        # value 是DO的值
        # 将DO状态上传IOA服务器

        try:
            bit_num = 0b00000000
            if value == 0:
                if bo_num == 0:
                    bit_num = 0b11111110
                elif bo_num == 1:
                    bit_num = 0b11111101
                elif bo_num == 2:
                    bit_num = 0b11111011
                elif bo_num == 3:
                    bit_num = 0b11110111
                elif bo_num == 4:
                    bit_num = 0b11101111
                elif bo_num == 5:
                    bit_num = 0b11011111
                elif bo_num == 6:
                    bit_num = 0b10111111
                elif bo_num == 7:
                    bit_num = 0b01111111
                BO = BO & bit_num
            elif value == 1:
                if bo_num == 0:
                    bit_num = 0b00000001
                elif bo_num == 1:
                    bit_num = 0b00000010
                elif bo_num == 2:
                    bit_num = 0b00000100
                elif bo_num == 3:
                    bit_num = 0b00001000
                elif bo_num == 4:
                    bit_num = 0b00010000
                elif bo_num == 5:
                    bit_num = 0b00100000
                elif bo_num == 6:
                    bit_num = 0b01000000
                elif bo_num == 7:
                    bit_num = 0b10000000
                BO = BO | bit_num
            return BO

        except Exception as e:
            print(e)

    def set_joint_angle(self, com_Pose = None,  speed=None, com_Num = None):
        if not self._running:
            raise RobotNotConnectedError("Controller not connected")

        if speed:
            self.speed = speed

        print(f"set_joint_angle {com_Pose[0]}")
        try:
            with self.lock:
                self.status = "PLANNING"
                if com_Num == 1:
                    path = self.planner.generate(

                        start = self.current_poseRobotA,
                        end = com_Pose[:6],
                        speed = self.speed
                    )
                elif com_Num == 2:
                    path = self.planner.generate(
                        start= self.current_poseRobotB,
                        end = com_Pose[:4],
                        speed = self.speed
                    )
                    
                self.trajectoryRobotA.queue.clear()
                self.trajectoryRobotB.queue.clear()

                for point in path:
                    if com_Num == 1:
                        self.trajectoryRobotA.put(point)
                    elif com_Num == 2:
                        self.trajectoryRobotB.put(point)
                
                self.target_pose = com_Pose
                self.status = "MOVING"
                
                ## 重置完成标志
                with self.condition:
                    self.movement_complete = False

                ## 等待运动完成
                with self.condition:
                    while not self.movement_complete:
                        # print(111111)
                        # time.sleep(0.1)
                        self.condition.wait()

        except Exception as e:
            raise MotionPlanningError(f"Path planning failed: {str(e)}")

    ## 机械臂复位（回零）
    def home(self, speed = None, com_Num= None):
        if not self._running:
            raise RobotNotConnectedError("Controller not connected")

        if speed:
            self.speed = speed

        com_Pose = [0.0,0.0,0.0,0.0,0.0,0.0,198.67,0.0,230.72,0.0,0.0,0.0,0.0]
        try:
            with self.lock:
                self.status = "PLANNING"
                if com_Num == 1:
                    path = self.planner.generate(

                        start = self.current_poseRobotA,
                        end = com_Pose[:6],
                        speed = self.speed
                    )
                elif com_Num == 2:
                    path = self.planner.generate(
                        start= self.current_poseRobotB,
                        end = com_Pose[:4],
                        speed = self.speed
                    )
                    
                self.trajectoryRobotA.queue.clear()
                self.trajectoryRobotB.queue.clear()

                for point in path:
                    if com_Num == 1:
                        self.trajectoryRobotA.put(point)
                    elif com_Num == 2:
                        self.trajectoryRobotB.put(point)
                
                self.target_pose = com_Pose
                self.status = "MOVING"
                
                # 重置完成标志
                with self.condition:
                    self.movement_complete = False

                # 等待运动完成
                with self.condition:
                    while not self.movement_complete:
                        time.sleep(0.5)
                        self.condition.wait()
            
        except Exception as e:
            raise MotionPlanningError(f"Path planning failed: {str(e)}")

    def stop(self):
        self._running = False
        self.status = "STOPPED"
        
        # 停止IOA服务并等待线程退出
        if hasattr(self, 'ioa_server'):
            self.ioa_server.stop()
        
        # 确保更新线程终止
        if self._update_thread.is_alive():
            self._update_thread.join(timeout=1)
            if self._update_thread.is_alive():
                robot_logger.warning("状态更新线程未正常退出")
        
        # 确保IOA服务线程终止
        if hasattr(self, 'algo_th') and self.algo_th.is_alive():
            self.algo_th.join(timeout=1)
            if self.algo_th.is_alive():
                robot_logger.warning("IOA服务线程未正常退出")
        
        logging.info("Controller stopped gracefully.")

    ## 机械臂状态更新（核心代码）
    def _state_updater(self):
        while self._running:
            # 更新RobotA的轨迹
            try:
                temp_pose = self.trajectoryRobotA.get_nowait()
                self.current_poseRobotA = temp_pose.copy()
                # 更新数据库中的关节值
                CL.set_value('j1', self.current_poseRobotA[0])
                CL.set_value('j2', self.current_poseRobotA[1])
                CL.set_value('j3', self.current_poseRobotA[2])
                CL.set_value('j4', self.current_poseRobotA[3])
                CL.set_value('j5', self.current_poseRobotA[4])
                CL.set_value('j6', self.current_poseRobotA[5])
            except queue.Empty:
                pass  # RobotA队列为空时不处理
            
            # 更新RobotB的轨迹
            try:
                temp_pose2 = self.trajectoryRobotB.get_nowait()
                self.current_poseRobotB = temp_pose2.copy()
                # 更新数据库中的关节值
                CL.set_value('j7', self.current_poseRobotB[0])
                CL.set_value('j8', self.current_poseRobotB[1])
                CL.set_value('j9', self.current_poseRobotB[2])
                CL.set_value('j10', self.current_poseRobotB[3])
            except queue.Empty:
                pass  # RobotB队列为空时不处理
            
            # 打印当前状态
            # print(f"current_poseRobotA: {self.current_poseRobotA}")
            # print(f"current_poseRobotB: {self.current_poseRobotB}")
            # print(f"target_pose: {self.target_pose}")
            
            target_a = self.target_pose[:6]
            target_b = self.target_pose[:4] 
            if (self.current_poseRobotA == target_a or self.current_poseRobotB == target_b):
                # print(22222222)
                with self.condition:
                    self.movement_complete = True
                    self.condition.notify_all()
                    
            time.sleep(0.1)  # 适当调整更新频率

    def _init_simulator(self):
        self.current_poseRobotA = [0.0] * 6
        self.current_poseRobotB = [0.0] * 4
        logging.info("Simulator initialized")

    def _connect_hardware(self):
        raise NotImplementedError("Hardware connection not implemented")

    # def __del__(self):
    #     self._running = False
    #     self.tcp.stop()
    #     if self._update_thread.is_alive():
    #         self._update_thread.join(timeout=5)

    def _run(self):
        ###  物流回仓指令(取料260，放料261)右上--右下--左下--左上
        cmd_logistics = self.slave.get_values("robot6", 260, 2)
        if cmd_logistics[0] == 1:
            time.sleep(1)
            self.set_joint_angle(com1_P = P0, com_Num=2)
            self.set_joint_angle(com_Pose= P26, com_Num=1)
            self.set_joint_angle(com_Pose= P27, com_Num=1)
            controller.SetDO(0, 1)
            self.set_joint_angle(com_Pose= P26, com_Num=1)
            controller.home(com_Num=1)
            self.set_joint_angle(com1_P =P0, com_Num=2)
            self.slave.set_values("robot6", 260, 0)
            self.master_.execute(1, cst.WRITE_SINGLE_REGISTER, starting_address=260, output_value=0)

        elif cmd_logistics[0] == 2:
            time.sleep(1)
            self.set_joint_angle(com1_P =P0, com_Num=2)
            self.set_joint_angle(com_Pose= P30, com_Num=1)
            self.set_joint_angle(com_Pose= P31, com_Num=1)
            controller.SetDO(0, 1)
            self.set_joint_angle(com_Pose= P30, com_Num=1)
            controller.home(com_Num=1)
            self.set_joint_angle(com1_P =P0, com_Num=2)
            self.slave.set_values("robot6", 260, 0)
            self.master_.execute(1, cst.WRITE_SINGLE_REGISTER, starting_address=260, output_value=0)
        
        elif cmd_logistics[0] == 3:
            time.sleep(1)
            self.set_joint_angle(com1_P =P0, com_Num=2)
            self.set_joint_angle(com_Pose= P34, com_Num=1)
            self.set_joint_angle(com_Pose= P35, com_Num=1)
            controller.SetDO(0, 1)
            self.set_joint_angle(com_Pose= P34, com_Num=1)
            controller.home(com_Num=1)
            self.set_joint_angle(com1_P =P0, com_Num=2)
            self.slave.set_values("robot6", 260, 0)
            self.master_.execute(1, cst.WRITE_SINGLE_REGISTER, starting_address=260, output_value=0)
        
        elif cmd_logistics[0] == 4:
            time.sleep(1)
            self.set_joint_angle(com1_P =P0, com_Num=2)
            self.set_joint_angle(com_Pose= P38, com_Num=1)
            self.set_joint_angle(com_Pose= P39, com_Num=1)
            controller.SetDO(0, 1)
            self.set_joint_angle(com_Pose= P38, com_Num=1)
            controller.home(com_Num=1)
            self.set_joint_angle(com1_P =P0, com_Num=2)
            self.slave.set_values("robot6", 260, 0)
            self.master_.execute(1, cst.WRITE_SINGLE_REGISTER, starting_address=260, output_value=0)
        

        if cmd_logistics[1] == 1:
            time.sleep(1)
            self.set_joint_angle(com_Pose= P28, com_Num=1)
            self.set_joint_angle(com_Pose= P29, com_Num=1)
            controller.SetDO(0, 0)
            time.sleep(0.5)
            self.set_joint_angle(com_Pose= P28, com_Num=1)
            self.set_joint_angle(com1_P =P0, com_Num=2)
            self.slave.set_values("robot6", 261, 0)
            self.master_.execute(1, cst.WRITE_SINGLE_REGISTER, starting_address=261, output_value=0)

        elif cmd_logistics[1] == 2:
            time.sleep(1)
            self.set_joint_angle(com_Pose= P32, com_Num=1)
            self.set_joint_angle(com_Pose= P33, com_Num=1)
            controller.SetDO(0, 0)
            time.sleep(0.5)
            self.set_joint_angle(com_Pose= P32, com_Num=1)
            self.set_joint_angle(com1_P =P0, com_Num=2)
            self.slave.set_values("robot6", 261, 0)
            self.master_.execute(1, cst.WRITE_SINGLE_REGISTER, starting_address=261, output_value=0)
        
        elif cmd_logistics[1] == 3:
            time.sleep(1)
            self.set_joint_angle(com_Pose= P36, com_Num=1)
            self.set_joint_angle(com_Pose= P37, com_Num=1)
            controller.SetDO(0, 0)
            time.sleep(0.5)
            self.set_joint_angle(com_Pose= P36, com_Num=1)
            self.set_joint_angle(com1_P =P0, com_Num=2)
            self.slave.set_values("robot6", 261, 0)
            self.master_.execute(1, cst.WRITE_SINGLE_REGISTER, starting_address=261, output_value=0)
        
        elif cmd_logistics[1] == 4:
            time.sleep(1)
            self.set_joint_angle(com_Pose= P40, com_Num=1)
            self.set_joint_angle(com_Pose= P41, com_Num=1)
            controller.SetDO(0, 0)
            time.sleep(0.5)
            self.set_joint_angle(com_Pose= P40, com_Num=1)
            self.set_joint_angle(com1_P =P0, com_Num=2)
            self.slave.set_values("robot6", 261, 0)
            self.master_.execute(1, cst.WRITE_SINGLE_REGISTER, starting_address=261, output_value=0)





class IOAServer:
    
    def __init__(self, controller):
        self.controller = controller
        self.tcp_server_socket = None
        self.address2 = ("0.0.0.0", 7070)
        self.connect_flag = 0
        # self.tcpServer()
        self._stop_event = threading.Event()

        self.connected_clients = []
        
        CL.set_value("BO0", 0)
        CL.set_value("BO1", 0)
        CL.set_value("FO[0]", self.float2bytes(75.0))
        CL.set_value("FO[1]", self.float2bytes(75.0))

    def tcpServer(self):
        self.tcp_server_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        self.tcp_server_socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
        self.tcp_server_socket.bind(self.address2)
        self.tcp_server_socket.listen(10)
        self.tcp_server_socket.settimeout(1) 

    ## 将浮点数打包成32位2进制数据
    def float2bytes(self,f):
        b=struct.pack('<f',f)        
        return b[0],b[1],b[2],b[3]  

    ## 检查总位数
    def checksum(self, Data, lenth):
        checksum_value=0
        data=list()
        for i in range(lenth-2):   #######从Data[2]开始，相应总长度减2
            data.append(Data[i+2])    
        for i in range(len(data)):
            checksum_value=((checksum_value+data[i]))%0xFFFF
        checksum_value_low=(checksum_value&0xFF)
        return checksum_value_low

    ## 打包发送数据--格式
    def set_sendData2IOA(self,):
        try:
            data = list()
            data.extend([0xff, 0xaa])
            data.append(0x01)
            data.extend([0x63, 0x00])
            data.extend([0x41, 0x00])
            data.extend([0x01, 0x05])

            # 子帧1: BI数据
            data.append(0x01)               #09
            data.extend([0x02,0x00])        #10,11
            data.extend([CL.get_value('BI0'),CL.get_value('BI1')])  #12,13

            # 子帧2: BO数据
            data.append(0x02)                           #14
            data.extend([0x02,0x00])                      #15,16       
            data.append(CL.get_value("BO0"))        #17
            # data.append(0xff)  
            data.append(CL.get_value("BO1"))        #18

            ###子帧（3）6个MS，float数据，来自ABB###
            data.append(0x03)                           #19
            data.extend([0x28,0x00])                    #20,21
            # with config_lock:

            if CL.get_value("j1") is not None:
                j0 = CL.get_value("j1")
                j1 = CL.get_value("j2")
                j2 = CL.get_value("j3")
                j3 = CL.get_value("j4")
                j4 = CL.get_value("j5")
                j5 = CL.get_value("j6")
            else:
                j0 = 0.0
                j1 = 0.0
                j2 = 0.0
                j3 = 0.0
                j4 = 0.0
                j5 = 0.0

            if CL.get_value("j7") is not None:
                j6 = CL.get_value("j7")
                j7 = CL.get_value("j8")
                j8 = CL.get_value("j9")
                j9 = CL.get_value("j10")
            else:
                    j6 = 0.0
                    j7 = 0.0
                    j8 = 0.0
                    j9 = 0.0

            # print(j0, j1, j2, j3, j4, j5, j6,j7,j8, j9)
            # BO = CL.get_value("BO0")
            # print(f"BO0 {BO}")

            data.extend(self.float2bytes(j0))         #22-25
            data.extend(self.float2bytes(j1))         #26-29
            data.extend(self.float2bytes(j2))        #30-33
            data.extend(self.float2bytes(j3))         #34-37
            data.extend(self.float2bytes(j4))         #38-41
            data.extend(self.float2bytes(j5))        #42-45  
            
            data.extend(self.float2bytes(j6))
            data.extend(self.float2bytes(j7))         #34-37
            data.extend(self.float2bytes(j8))        #38-41
            data.extend(self.float2bytes(j9))  
            
            # 子帧4: FI数据
            data.append(0x84)                           #46
            data.extend([0x10,0x00])                    #47,48
            data.extend(CL.get_value("FI[0]"))      #49-52
            data.extend(CL.get_value("FI[1]"))      #53-56
            data.extend(CL.get_value("FI[2]"))      #57-60
            data.extend(CL.get_value("FI[3]"))      #61-64
            
            data.append(0x85)                           #65
            data.extend([0x10,0x00])     
            data.extend(CL.get_value("FO[0]"))      #FO 0
            data.extend(CL.get_value("FO[1]"))      #FO 0
            data_FO=[0x00]*8

            data.extend(data_FO)

            # 计算校验和
            data.append(self.checksum(data, 100))
            data.extend([0xff, 0x55])
            # print(data)
            return data
        
        except Exception as e:
            robot_logger.error(f"Data packing error: {str(e)}")
            return None

    def calculate_recvdata(self,recv_value,lenth):
        
        data=list()                       
        for i in range(0,lenth):
            data.append(recv_value[i])
        # print('接收到IOA数据')
        CL.set_value("BI0",data[12])    ###16位BI数据中低8位BI0----BI7                    
        CL.set_value("BI1",data[13])    ###16位BI数据中高8位BI8----BI15
        ###4个一组为一个FI，共4个FI,data[17]-data[32]
        tf0=(data[17:21])
        tf1=(data[21:25])
        tf2=(data[25:29])
        tf3=(data[29:33])
        CL.set_value("FI[0]",value=tf0)
        CL.set_value("FI[1]",value=tf1) ##FI.append(data[17+4*i:21+4*i])
        CL.set_value("FI[2]",value=tf2)
        CL.set_value("FI[3]",value=tf3)  

    def stop(self):
        self._stop_event.set()  # 触发停止事件
        if self.tcp_server_socket:
            try:
                self.tcp_server_socket.close()
            
            except Exception as e:
                robot_logger.error(f"关闭服务端套接字错误: {str(e)}")
            self.tcp_server_socket = None
        
        for client in self.connected_clients:
            try:
                client.close()
            
            except Exception as e:
                robot_logger.error(f"关闭客户端套接字错误: {str(e)}")
        self.connected_clients.clear()

    def run(self):
        while not self._stop_event.is_set():
            try:
                if not self.tcp_server_socket:
                    self.tcpServer()  # 确保套接字初始化
                
                try:
                    self.connect_flag = 0
                    self.client_socket, addr = self.tcp_server_socket.accept()
                    print("from:",addr)
                    time.sleep(0.1)
                    self.client_socket.settimeout(1)
                    self.connected_clients.append(self.client_socket)
                    self.connect_flag = 1
                    while not self._stop_event.is_set():
                        try:
                            recv_data=self.client_socket.recv(1024)
                            if len(recv_data)<36:  ##############处理接收数据
                                print(f"recv_data less then 36")
                                # client_socket,client_Addr=self.tcp_server_socket.accept()
                                # pass
                                break
                            else:
                                for i in range(0,len(recv_data)):
                                    if (recv_data[i]==0xff and recv_data[i+1]==0xaa):
                                        recv_data=recv_data[i:]
                                        break                  
                                if (recv_data[0]==0xff and recv_data[1]==0xaa):
                                    if (recv_data[8]==0x02):
                                        if (recv_data[9]==0x01 and recv_data[10]==0x02 and recv_data[14]==0x84 and recv_data[15]==0x10):
                                            self.calculate_recvdata(recv_data,36)
                                            senddata=self.set_sendData2IOA()
                                            send_data=bytes(senddata)
                                            # print(f"send to IOA data {send_data}")                                   
                                            self.client_socket.send(send_data)                                                               
                                        else:
                                            print("congfig参数不对")
                                    else:
                                        print("外设数目不为2")                            
                                else:
                                    print('报头校验不通过')
                        
                        except Exception as e:
                            robot_logger.error(f"Accept error: {str(e)}")

                    self.client_socket.close()
                    self.connected_clients.remove(self.client_socket)

                except socket.timeout:
                    continue
                except Exception as e:
                    if not self._stop_event.is_set():
                        robot_logger.error(f"accept connect failed {str(e)}")
                    break

            except Exception as e:
                if not self._stop_event.is_set():
                    robot_logger.error(f"Accept2 error: {str(e)}")





### 示例代码程序（测试专用）
if __name__ == "__main__":
    logging.basicConfig(level=logging.DEBUG)
    controller = VSRController(simulate=True)
    
    try:
        controller.connect()
        time.sleep(1)
        controller.set_joint_angle([10.0, 20.0, 0.0, 0.0, 0.0, 0.0, 10, 20, 0, 0])
        time.sleep(1)
        controller.set_joint_angle([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0, 0, 0])
        time.sleep(3)
        controller.set_joint_angle([50.0, 0.0, 0.0, 0.0, 0.0, 0.0, 10, 0, 0, 0])
        time.sleep(1)
        controller.set_joint_angle([50.0, 10.0, 0.0, 0.0, 0.0, 0.0, 10, 10, 0, 0])
        time.sleep(1)
        controller.set_joint_angle([50.0, 0.0, 10.0, 0.0, 0.0, 0.0, 10, 10, 10, 0])
        time.sleep(1)
        # while True:
        #     time.sleep(1)
    
    except KeyboardInterrupt:
        print("\nShutting down controller service...")
        controller.stop()
    # finally:
    #     controller.stop()